#include "skeleton.h"

Skeleton::Skeleton() {
}

Skeleton::Skeleton(const Skeleton& s) {
   rootPosition = s.rootPosition;
   trOrder = s.trOrder;
   bones = s.bones;
   hierarchy = s.hierarchy;
   father = s.father;
}

Skeleton::Skeleton(Point3D rp, Hierarchy h) {
   rootPosition = rp;
   hierarchy = h;
}

Skeleton::~Skeleton() {
}

void Skeleton::insertSon(string bone, string son) {
   hierarchy[bone].push_back(son);
   father[son] = bone;
}

void Skeleton::insertBone(Bone* b) {
   bones[b->getName()] = *b;
}

void Skeleton::rotateBone(string bone, Vector3D angles, string order,
      bool isRoot, bool isDeg) {

   string n = bone;
   queue<string> Q;
   Q.push(n);
   Sons sons;

   Vector3D axisX(1.0, 0.0, 0.0);
   Vector3D axisY(0.0, 1.0, 0.0);
   Vector3D axisZ(0.0, 0.0, 1.0);
   
//   Vector3D axisX = make_vector(1.0, 0.0, 0.0);
//   Vector3D axisY = make_vector(0.0, 1.0, 0.0);
//   Vector3D axisZ = make_vector(0.0, 0.0, 1.0);

   if (!isRoot) {
      axisX = bones[n].getAxisX();
      axisY = bones[n].getAxisY();
      axisZ = bones[n].getAxisZ();
   }

   while (!Q.empty()) {
      n = Q.front();
      Q.pop();
      
      Vector3D rot = bones[n].getDirection();

      for (int i = 0; i < (int) order.size(); i++) {
         if (order[i] == 'X' && angles[0] != 0) {
            rot = rotate(angles[0], rot, axisX, isDeg);
            bones[n].setAxisX(rotate(angles[0], bones[n].getAxisX(), axisX, isDeg));
            bones[n].setAxisY(rotate(angles[0], bones[n].getAxisY(), axisX, isDeg));
            bones[n].setAxisZ(rotate(angles[0], bones[n].getAxisZ(), axisX, isDeg));
         } else if (order[i] == 'Y' && angles[1] != 0) {
            rot = rotate(angles[1], rot, axisY, isDeg);
            bones[n].setAxisX(rotate(angles[1], bones[n].getAxisX(), axisY, isDeg));
            bones[n].setAxisY(rotate(angles[1], bones[n].getAxisY(), axisY, isDeg));
            bones[n].setAxisZ(rotate(angles[1], bones[n].getAxisZ(), axisY, isDeg));
         } else if (order[i] == 'Z' && angles[2] != 0) {
            rot = rotate(angles[2], rot, axisZ, isDeg);
            bones[n].setAxisX(rotate(angles[2], bones[n].getAxisX(), axisZ, isDeg));
            bones[n].setAxisY(rotate(angles[2], bones[n].getAxisY(), axisZ, isDeg));
            bones[n].setAxisZ(rotate(angles[2], bones[n].getAxisZ(), axisZ, isDeg));
         }
      }

      bones[n].setDirection(rot);

      // Refreshing queue
      sons = getSons(n);
      for (int i = 0; i < (int) sons.size(); i++) {
         Q.push(sons[i]);
      }
   }
}

Segment3D Skeleton::boneSegment(int i) {
   Segment3D s;
   map<string, Bone>::iterator it = bones.begin();
   advance(it, i);
   
   string currentBone = it->second.getName();
   string currentFather = father[currentBone];
   
//   cout << currentBone << "|-----|" << currentFather << endl;
   
   s.source = point(currentBone);
   s.target = point(currentFather);
   return s; 
}

Point3D Skeleton::point(string bone){
   if (bone == "root") {
      return rootPosition;
   } else {
      return point(father[bone]) + bones[bone].lenXdir();
   }
}
